#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
import rclpy.qos as qos
from sls_msgs.msg import LlaVelocity,StateUGV
from sls_msgs.srv import SetLocalOrigin
from nav_msgs.msg import Odometry
import math



def lla2ecef(lon, lat, alt):
    """
    unit of inputs should be rad
    """
    a = 6378137.0  # a axis
    f = 1 / 298.257223565  # f = (a-b)/a
    e_square = f * (2 - f)
    N = a / math.sqrt(1 - e_square * math.sin(lat) ** 2)
    X = (N + alt) * math.cos(lat) * math.cos(lon)
    Y = (N + alt) * math.cos(lat) * math.sin(lon)
    Z = (N * (1 - e_square) + alt) * math.sin(lat)

    return [X, Y, Z]


def lla2enu(lon, lat, alt, lon0, lat0, alt0):
    p0 = lla2ecef(lon0, lat0, alt0)
    p = lla2ecef(lon, lat, alt)
    [u, v, w] = [p[0] - p0[0], p[1] - p0[1], p[2] - p0[2]]
    cos_lon0 = math.cos(lon0)
    sin_lon0 = math.sin(lon0)
    cos_lat0 = math.cos(lat0)
    sin_lat0 = math.sin(lat0)

    e = -sin_lon0 * u + cos_lon0 * v
    t = cos_lon0 * u + sin_lon0 * v

    n = -sin_lat0 * t + cos_lat0 * w
    u = cos_lat0 * t + sin_lat0 * w
    # if e << 1, there is the approximation:
    # e = a * math.cos(lat)*delta_lon
    # n = a * delta_lat
    # u = delta_alt
    return [e, n, u]


def deg2rad(x):
    return x * math.pi / 180.0


class RtkInfo(Node):
    def __init__(self, name):
        super().__init__(name)
        self.origin = None
        self.enu = None
        self.vel_yaw = None
        self.ugv_yaw = None
        self.ugv_vel = None

        self.origin_lon = None
        self.origin_lat = None
        self.origin_alt = None
        self.ned = None

        self.lla_sub = self.create_subscription(LlaVelocity, "/ugv/sensors/rtk/out/lla", self.get_rkt_info, qos.qos_profile_sensor_data)
        self.ugv_state_sub = self.create_subscription(StateUGV,"/ugv/state/msg/out/ugv_state",self.ugv_state_callback,10)
        self.odom_pub = self.create_publisher(Odometry, "/ugv/out/odom", 10)
        self.set_origin_srv = self.create_service(SetLocalOrigin, "/ugv/sensors/rtk/in/set_origin", self.set_local_origin)

    def set_local_origin(self, request:SetLocalOrigin.Request, response:SetLocalOrigin.Response):
        self.origin_lon = request.origin_lon
        self.origin_lat = request.origin_lat
        self.origin_alt = request.origin_alt
        response.ok = True
        return response

    def get_rkt_info(self, msg:LlaVelocity):
        altitude = msg.altitude
        latitude = msg.latitude
        longitude = msg.longitude
        lon = deg2rad(longitude)
        lat = deg2rad(latitude)
        alt = deg2rad(altitude)

        self.ugv_vel = msg.vel

        vel_heading = msg.vel_heading
        vel_heading = deg2rad(((360.0 - vel_heading) + 90) % 360.0)

        self.ugv_yaw = msg.yaw
        self.ugv_yaw = deg2rad(((360.0 - self.ugv_yaw) + 90) % 360.0)

        self.origin_lon = 119.5036822
        self.origin_lat = 32.1991817
        self.origin_alt = 0.0
        self.origin = 1
        if self.origin is None:
            self.enu = None
        else:
            self.enu = lla2enu(
                lon,
                lat,
                alt,
                deg2rad(self.origin_lon),
                deg2rad(self.origin_lat),
                deg2rad(self.origin_alt),
            )
            # e,n,u = self.enu
            # self.ned = n, e, -u
        return

    def ugv_state_callback(self,msg:StateUGV):
        if self.enu is None:
            return
        else:
            direction = msg.steering_wheel_direction
            angle = msg.steering_wheel_angle/120*20/180*3.1415926536
            angle = -angle if direction == 0 else angle
            yaw = self.ugv_yaw

            speed = msg.vehicle_speed


            nav2_msg = Odometry()
            nav2_msg.header.stamp = self.get_clock().now().to_msg()
            nav2_msg.header.frame_id = "odom"
            nav2_msg.child_frame_id = "base_footprint"

            nav2_msg.pose.pose.position.x,nav2_msg.pose.pose.position.y,nav2_msg.pose.pose.position.z = self.enu
            
            nav2_msg.pose.pose.orientation.x = 0.0
            nav2_msg.pose.pose.orientation.y = 0.0
            nav2_msg.pose.pose.orientation.z = math.sin(yaw/2)
            nav2_msg.pose.pose.orientation.w = math.cos(yaw/2)

            # speed/self.ugv_vel
            nav2_msg.twist.twist.linear.x = speed * math.cos(yaw)
            nav2_msg.twist.twist.linear.y = speed * math.sin(yaw)
            nav2_msg.twist.twist.linear.z = 0.0

            nav2_msg.twist.twist.angular.x = 0.0
            nav2_msg.twist.twist.angular.y = 0.
            nav2_msg.twist.twist.angular.z = speed * math.tan(angle) / 0.9
            self.odom_pub.publish(nav2_msg)
            # print(nav2_msg)
        return


def main():
    rclpy.init()
    rclpy.spin(RtkInfo(name="calc_local_position"))
    rclpy.shutdown()
